# 研究机构：上海大学机自学院
# 研 究 生：王强
# 开发时间：2024/6/6 20:39
import numpy as np
import pandas as pd
import math
import matplotlib.pyplot as plt

class path_generation:
    def __init__(self, T, w):
        # G_v = np.array([[T * T / 2, 0],
        #             [0, 0],
        #             [0, T * T / 2],
        #             [0, 0]])
        self.Q_k = np.diag([3**2, 0, 3**2, 0])
        # self.Q_k = np.dot(np.dot(G_v, q_v), np.transpose(G_v))                   # 过程协方差矩阵
        print(self.Q_k)
        self.W_q = [0, 0, 0, 0]                                #
        self.W_v = [0, 0]                                #
        self.x_ture = []                                    # 当前时刻
        self.x_ture_ = []                                 # 下一时刻
        # self.v_obs = []                                  # 雷达观测值
        self.V_k = np.diag([10 ** 2, 1 ** 2])             # 观测噪声协方差矩阵
        self.F = np.array([[1, math.sin(w * T) / w, 0, -(1 - math.cos(w * T)) / w],
                           [0, math.cos(w * T), 0, -math.sin(w * T)],
                           [0, (1 - math.cos(w * T)) / w, 1, math.sin(w * T) / w],
                           [0, math.sin(w * T), 0, math.cos(w * T)]])

    def radar_observation(self):
        # print(self.x_ture)
        # print(self.Q_k)
        self.x_ture_ = np.dot(self.F, np.transpose(self.x_ture)) + np.random.multivariate_normal(self.W_q, self.Q_k)
        print('111',self.x_ture_)
        radius = np.linalg.norm([self.x_ture[0], self.x_ture[2]], ord=2)
        azimuth = math.atan2(self.x_ture[2], self.x_ture[0]) / 3.14 * 180
        z_obs = [radius, azimuth] + np.random.multivariate_normal(self.W_v, self.V_k)
        return z_obs


total = 120
X = [[3500, -15, 2000, 15],
     [3000, -15, 1000, 15],
     [2000, -15, 500, 15]]
w = [3.14 / 180 * 2, -3.14 / 180 * 1, -3.14 / 180 * 1]
X_ture = np.zeros((total, len(X)))
Y_ture = np.zeros((total, len(X)))
Z_obs_dis = np.zeros((total, len(X)))
Z_obs_ath = np.zeros((total, len(X)))

for j in range(len(X)):
    path_i = path_generation(T=1, w=w[j])
    for i in range(total):
        if i==0:
            path_i.x_ture = X[j]
        else:
            path_i.x_ture = path_i.x_ture_
        z_obs_i = path_i.radar_observation()
        X_ture[i, j] = path_i.x_ture[0]
        Y_ture[i, j] = path_i.x_ture[2]
        Z_obs_dis[i, j] = z_obs_i[0]
        Z_obs_ath[i, j] = z_obs_i[1]
# 存储
df = pd.DataFrame(X_ture, columns=['x_1', 'x_2', 'x_3'])
df.to_excel('x_ture.xlsx', index=False)

df = pd.DataFrame(Y_ture, columns=['y_1', 'y_2', 'y_3'])
df.to_excel('y_ture.xlsx', index=False)

df = pd.DataFrame(Z_obs_dis, columns=['dis_1', 'dis_2', 'dis_3'])
df.to_excel('dis_obs.xlsx', index=False)

df = pd.DataFrame(Z_obs_ath, columns=['ath_1', 'ath_2', 'ath_3'])
df.to_excel('ath_obs.xlsx', index=False)

# 绘图
plt.figure()
# plt.plot(X_ideal[::, 0], X_ideal[::, 1], 'r-+')
for i in range(len(X)):
    plt.plot(X_ture[::, i], Y_ture[::, i], 'b-+')
    x_obs = Z_obs_dis[::, i] * np.cos(Z_obs_ath[::, i]/180*3.14)
    y_obs = Z_obs_dis[::, i] * np.sin(Z_obs_ath[::, i]/180*3.14)
    plt.plot(x_obs, y_obs, 'g-+')
plt.show()

